#include "FundamentalMat.hpp"
#include "../VisionTools.hpp"
#include "CoordNormalizer.hpp"
#include "Triangulator.hpp"
#include <zMat.hpp>

namespace zzz{
bool FundamentalMat::Create8(const vector<Vector2d> &ls, const vector<Vector2d> &rs)
{
  //implementation of 11.1
  assert(ls.size()==rs.size());

  //1. normalize
  CoordNormalizer norl(ls),norr(rs);

  //2. 8 point linear
  zMatrix<double> A(ls.size(),9),U,S,VT;
  for (zuint i=0; i<ls.size(); i++)
  {
    Vector2d l=ls[i];
    norl.Normalize(l);
    Vector2d r=rs[i];
    norr.Normalize(r);
    A(i,0)=l[0]*r[0];
    A(i,1)=l[0]*r[1];
    A(i,2)=l[0];
    A(i,3)=l[1]*r[0];
    A(i,4)=l[1]*r[1];
    A(i,5)=l[1];
    A(i,6)=r[0];
    A(i,7)=r[1];
    A(i,8)=1;
  }

  //TODO: optimizable, use dgelsy or dgesv
  zMatrix<double> A2(A);
  bool svdres=SVD(A,U,S,VT);
  ZCHECK(svdres)<<"SVD failed when finding Fundamental Matrix!";
  zMatrix<double> F(3,3);
  F(0,Colon())=VT(8,Colon(0,2));
  F(1,Colon())=VT(8,Colon(3,5));
  F(2,Colon())=VT(8,Colon(6,8));
  
  //3. make order to 2
//  Matrix3x3d F1;
//  Dress(F1)=F;
//  zout<<"F="<<F1<<endl;
  svdres=SVD(F,U,S,VT);
  ZCHECK(svdres)<<"SVD failed when finding Fundamental Matrix!";
  zMatrix<double> DS(Diag(S));
  DS(2,2)=0;
  Matrix3x3d F2;
  Dress(F2)=U*DS*VT;
//  zout<<"F'="<<F2<<endl;

  //4. restore
  *this=norl.GetT().Transposed()*F2*norr.GetT();

  *this/=(*this)(2,2);

  //CHECK
//  for (zuint i=0; i<ls.size(); i++)
//  {
//    zout<<"CHECK X'FX="<<(ToHomogeneous(ls[i])*(*this)).Dot(ToHomogeneous(rs[i]))<<endl;
//    zout<<"CHECK residue="<<Residue(ls[i],rs[i])<<endl;
//  }
  return true;
}

int FundamentalMat::Create8RANSAC(const vector<Vector2d> &ls, const vector<Vector2d> &rs, const double outlier_threshold, IterExitCond<double> &cond)
{
  cond.Reset();
  int inliers, max_inliers=0;
  RANSACPicker<8> picker(0,ls.size()-1);
  Matrix3x3d best_me;
  do
  {
    //randomly pick 8 point
    Vector<8,int> pick=picker.Pick();
    //calculate f
    vector<Vector2d> lls,rrs;
    for (zuint i=0; i<8; i++)
    {
      lls.push_back(ls[pick[i]]);
      rrs.push_back(rs[pick[i]]);
    }
    Create8(lls,rrs);
    //check inliers
    inliers=0;
    for (zuint i=0; i<ls.size(); i++)
    {
      double resi=Residue(ls[i],rs[i]);
//      zout<<i<<" residue "<<resi<<endl;
      if (resi<outlier_threshold)
        inliers++;
    }
    if (inliers>max_inliers)
    {
      best_me=(*this);
      max_inliers=inliers;
    }
  }while(!cond.IsSatisfied(double(ls.size()-inliers)/ls.size()));
  Matrix3x3d::operator=(best_me);

  //refine by all inliers
  vector<Vector2d> lls,rrs;
  for (zuint i=0; i<ls.size(); i++)
  {
    if (Residue(ls[i],rs[i])<outlier_threshold)
    {
      lls.push_back(ls[i]);
      rrs.push_back(rs[i]);
    }
  }
  if (lls.size()>=8) 
    Create8(lls,rrs);
  return inliers;
}

int FundamentalMat::Create8RANSAC(PairData<double> &data, const double outlier_threshold, IterExitCond<double> &cond)
{
  const vector<Vector2d> &ls=data.pos2ds[1],&rs=data.pos2ds[0];
  Create8RANSAC(ls,rs,outlier_threshold,cond);

  int x=0;
  for (zuint i=0; i<ls.size(); i++)
  {
    data.ferror[i]=Residue(ls[i],rs[i]);
    if (data.ferror[i]>outlier_threshold)
      data.status[i]=POINT_BADF;
    else 
      x++;
  }
  return x;
}

bool FundamentalMat::CalEpipoles(Vector3d &left, Vector3d &right)
{
  zMatrixd A(Dress(*this)),U,S,VT;
  bool svdres=SVD(A,U,S,VT);
  ZCHECK(svdres)<<"SVD failed when calculating epipoles!";
  Dress(left)=Trans(VT(2,Colon()));
  Dress(right)=U(Colon(),2);
  return true;
}

double FundamentalMat::Residue(const Vector2d &l, const Vector2d &r) const
{
//  return abs((ToHomogeneous(l)*(*this)).Dot(ToHomogeneous(r)));

  Vector3d fl=ToHomogeneous(l)*(*this);
  Vector3d fr=(*this)*ToHomogeneous(r);

  double pt=r[0]*fl[0]+r[1]*fl[1]+fl[2];

  return (1.0 / (fl[0]*fl[0] + fl[1]*fl[1]) +
        1.0 / (fr[0]*fr[0] + fr[1]*fr[1])) *
      (pt * pt);
}



/////////////////////////////////////////////////////////////
bool EssentialMat::GetProjection(ProjectionMatd &P, const Vector2d &l, const Vector2d &r) const
{
  ZLOG(ZDEBUG)<<SplitLine("Get Projection From Essential Matrix")<<endl;

  vector<Vector2d> pos2ds;
  pos2ds.push_back(r);
  pos2ds.push_back(l);

  vector<ProjectionMatd> PP(2,ProjectionMatd());
  PP[0].Identical();

  //SVD of E to find R
  zMatrix<double> A(Dress(*this)),U,S,VT;
  bool svdres=SVD(A,U,S,VT);
  ZCHECK(svdres)<<"SVD failed when decompose E";
  double s=Sqrt(Sqrt(S(0,0)*S(1,0)));
  ZLOG(ZDEBUG)<<"S=\n"<<S<<endl;

  //two possibilities of R
  Matrix3x3d W(0,-1,0, 1,0,0, 0,0,1),Z(0,1,0, -1,0,0, 0,0,0);
  Matrix3x3d R[4];
  Dress(R[0])=U*Dress(W)*VT;
  R[0]*=Sign(R[0].Determinant());  //if R=-R, everything will fit, but camera direction will flip, which impossible (two cameras wouldn't shot same points)
  R[1]=R[0];
  Dress(R[2])=U*Trans(Dress(W))*VT;
  R[2]*=Sign(R[2].Determinant());
  R[3]=R[2];
  ZLOG(ZDEBUG)<<"R1=\n"<<R[0]<<endl;
  ZLOG(ZDEBUG)<<"R2=\n"<<R[2]<<endl;

  //t=u3
  Vector3d u3(U(0,2),U(1,2),U(2,2));
  Vector3d t[4]={u3,-u3,u3,-u3};
  ZLOG(ZDEBUG)<<"t="<<t[0]<<endl;

  //choose
  ZLOG(ZDEBUG)<<"l="<<l<<" r="<<r<<endl;
  int chosen;
  for (zuint i=0; i<4; i++)
  {
    bool good=true;
    PP[1].SetRT(R[i],t[i]);
    PP[1].MakeP();
    ZLOG(ZDEBUG)<<"CC="<<PP[1].CameraCenter()<<endl;
    ZLOG(ZDEBUG)<<"PR="<<PP[1].PrincipalRay()<<endl;
    
    Vector3d X=Triangulator::LinearTriangulate(PP,pos2ds);
    ZLOG(ZDEBUG)<<"X="<<X<<endl;
    string msg;
    if (X[2]>0)
      msg<<i<<" X is in front of P1\t";
    else
    {
      good=false;
      msg<<i<<" X is NOT in front of P1\t";
    }


    //camera center and orientation of camera
    double tmp=PP[1].PrincipalRay().Dot(X-PP[1].CameraCenter());
    if (tmp>0)
      msg<<i<<" X is in front of P2";
    else
    {
      good=false;
      msg<<i<<" X is NOT in front of P2";
    }
    if (good) chosen=i;
    ZLOG(ZDEBUG)<<msg<<endl;
  }
  ZLOG(ZDEBUG)<<endl;

  ZLOG(ZDEBUG)<<chosen<<" is chosen\n";
  ZLOG(ZVERBOSE)<<"R=\n"<<R[chosen]<<endl;
  ZLOG(ZVERBOSE)<<"t="<<t[chosen]<<endl;
  P.SetRT(R[chosen],t[chosen]);
  P.MakeP();
  return true;
}

}